#pragma once

#include "vio/front_end/feature.h"
#include "vio/front_end/frame.h"
#include "vio/front_end/point.h"
#include "vio/vio_common.h"

namespace vio {

// contianer for converged 3D points that are not already to two keyframes
class MapPointCandidates {
 public:
  typedef std::shared_ptr<MapPointCandidates> Ptr;
  typedef std::pair<Point::Ptr, Feature::Ptr> PointCandidate;
  typedef std::list<PointCandidate> PointCandidateList;

  // the depth filter is running in a parallel and fills the candidate
  // list. this mutex controls concurrent access to point_candidates.
  boost::mutex mutex_candidate_;

  // candidate points are created from converged seeds.
  // until the next keyframe, these points can be used for reprojection and
  // pose optimization.
  PointCandidateList candidates_;
  std::list<Point::Ptr> trash_points_;

  MapPointCandidates();

  ~MapPointCandidates();

  // add a candidate point
  void NewCandidatePoint(Point::Ptr point, double depth_sigma2);

  // add the feature to the frame and deletes candidates from list
  void AddCandidatePointToFrame(Frame::Ptr frame);

  // remove a candidate point from the list of candidates
  bool DeleteCandidatePoint(Point::Ptr point);

  // remove all candidates that belong to a frame
  void RemoveFrameCandidates(Frame::Ptr frame);

  // reset the candidate list, remove and delete all points
  void Reset();

  void DeleteCandidate(PointCandidate& c);

  void EmptyTrash();
};

// map object which saves all keyframes which are in a map
class Map : boost::noncopyable {
 public:
  typedef std::shared_ptr<Map> Ptr;
  std::list<Frame::Ptr> keyframes_;  // list of keyframes in the map
  std::list<Point::Ptr>
      trash_points_;  // a deleted point is moved to the trash
                      // bin. now and then this is cleaned. one reason is that
                      // visualizer must remove the points also
  MapPointCandidates point_candidates_;

  Map();

  ~Map();

  // reset the map. delete all keyframes and reset the frame and point counters
  void Reset();

  // delete a point in the map and remove all references in keyframes to it.
  void SafeDeletePoint(Point::Ptr pt);

  // moves the point to the trash queue which is cleaned now and then.
  void DeletePoint(Point::Ptr point);

  // moves the frame to the trash queue which is cleaned now and then.
  bool SafeDeleteFrame(Frame::Ptr frame);

  // remove the references between a point and a frame.
  void RemovePtFrameRef(Frame::Ptr frame, Feature::Ptr ftr);

  // add a new keyframe to the map
  void AddKeyFrame(Frame::Ptr new_kf);

  // given a frame, return all keyframe which have an overlapping field of view.
  void GetCloseKeyFrames(
      const Frame::Ptr& frame,
      std::list<std::pair<Frame::Ptr, double>>& close_kfs) const;

  // return the keyframe which is spatially closest and has overlapping field of
  // view.
  Frame::Ptr GetClosestKeyFrame(const Frame::Ptr& frame) const;

  // return the keyframe which is further apart from pos.
  Frame::Ptr GetFurthestKeyFrame(const Eigen::Vector3d& pos) const;

  bool GetKeyFrameById(const int id, Frame::Ptr& frame) const;

  // transform the whole map with rotation R, translation t and scale s.
  void Transform(const Eigen::Matrix3d& R,
                 const Eigen::Vector3d& t,
                 const double& s);

  // empty trash bin of deleted keyframes and map points. we don't delete the
  // points immediately to ensure proper cleanup and to provide the visualizer
  // a list of objects whitch must be removed.
  void EmptyTrash();

  // return the keyframe which was last inserted in the map
  inline size_t size() const {
    return keyframes_.size();
  }
};

}  // namespace vio
